#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Vector3.h>

ros::Publisher pub_;

void eventCallback(const nav_msgs::Odometry &reading)
{
  geometry_msgs::Vector3 ypr;
  geometry_msgs::Quaternion q= reading.pose.pose.orientation;

  ypr.x = atan2(q.w*q.x+q.y*q.z, 0.5-q.x*q.x-q.y*q.y);
  ypr.y = asin(2*(q.w*q.y - q.x*q.z));
  ypr.z = atan2(q.w*q.z+q.x*q.y, 0.5-q.y*q.y-q.z*q.z);
  /*
  ypr.x = atan2( q.x*q.y - q.w*q.z, q.w*q.w+q.x*q.x-0.5 );
  ypr.y = -asin( 2* (q.x*q.z + q.w*q.y) );
  ypr.z = atan2( q.y*q.z - q.w*q.z, q.w*q.w+q.z*q.z-0.5 );
  */
  //ROS_INFO("YAW= %lf", ypr.z);
  pub_.publish(ypr);
}

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "odom_listener");
  ros::NodeHandle nh;

  pub_ = nh.advertise<geometry_msgs::Vector3>("ypr", 100);
  ros::Subscriber sub = nh.subscribe("Odom", 10, eventCallback);
  
  ros::spin();
}
